package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.logic.costfunction.CostConstants;
import com.googlecode.curvedmetromaps.core.model.MetroEdge;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Constraint S4: forces that push edges apart.
 */
public class ForceS4 extends ForceSubsystem {

	ForceS4(ForceSystem system) {
		super(system);
	}
	
	void unitTest() {
		Vec2[] triangle = new Vec2[3];
		for (int i = 0; i < 3; i++)
			triangle[i] = new Vec2(
				(float) (Math.floor(Math.random() * 100.)), 
				(float) (Math.floor(Math.random() * 100.)));

		int t = (int) Math.floor(3.0 * Math.random());
		
		Vec2 a = triangle[t];
		Vec2 b = triangle[(t + 1) % 3];
		Vec2 c = triangle[(t + 2) % 3];
		Vec2 ba = a.sub(b);
		Vec2 bc = c.sub(b);

		double at2_ba = Math.atan2(ba.y, ba.x);	
		double at2_bc = Math.atan2(bc.y, bc.x);	
		double theta = (at2_bc - at2_ba) - Math.PI;
		// Move the range to [-pi, pi).
		if (theta <= -Math.PI)
			theta += 2f * Math.PI;
		
		Vec2 edge = b.sub(a);
		Vec2 solution = a.add(edge.mul((float) Math.random()));
		
		Vec2 y = new Vec2(-edge.y, edge.x);
		if (theta > 0)
			y.negateLocal();
		y.mulLocal((float) (Math.random() * 2.));
		y.addLocal(solution);

		Vec2 sqDist = squaredDistance(y, triangle[0], triangle[1], triangle[2]);
		Vec2 sqDistPlusY = y.add(sqDist); 
		double error = Math.round(sqDistPlusY.sub(solution).length() * 1000.) / 1000.;
		System.out.println("error=" + String.valueOf(error));
		
		System.out.println("y={" + String.valueOf(y.x) + "," + String.valueOf(y.y) + "};");
		System.out.println("solution={" + String.valueOf(solution.x) + "," + String.valueOf(solution.y) + "};");
		System.out.print("Graphics[{");

		// Print y-->solution
		System.out.print("Arrow[{y,solution}],");
		// Print triangle
		System.out.print("Arrow[{");
		for (int i = 0; i < 3; i++) {
			System.out.print("{" + String.valueOf(triangle[i].x) + "," + String.valueOf(triangle[i].y) + "}, ");
		}
		System.out.print("{" + String.valueOf(triangle[0].x) + "," + String.valueOf(triangle[0].y) + "}");
		System.out.print("}],Green,");
		// Print y-->sqDist.add(y)
		System.out.print("Arrow[{y,{" + String.valueOf(sqDistPlusY.x) + "," + String.valueOf(sqDistPlusY.y) + "}}]");
		System.out.println("}]");
		
		// Debug
		squaredDistance(y, triangle[0], triangle[1], triangle[2]);
	}

	void applyForces(int delta) {
		unitTest();
		
		for (MetroStation m : system.metroMap.stations) {
			int mid = m.id;
			Body mbody = system.stationBodies[mid];
			Vec2 mpos = mbody.getPosition();
			for (MetroEdge e : system.metroMap.edgeArray) {
				if (e.src.id != mid && e.dst.id != mid) {
					Vec2 v0 = system.stationBodies[e.src.id].getPosition();
					Vec2 v1 = system.stationBodies[e.dst.id].getPosition();
					Vec2 v2 = new Vec2(e.cpX, e.cpY);
					Vec2 direction = squaredDistance(mpos, v0, v1, v2);
					float squaredist = direction.lengthSquared();
					if (squaredist < CostConstants.S4_DEMIN) {
						float dist = direction.normalize();
						final float c = -CostConstants.S4_LAMBDA / CostConstants.S4_DEMIN;
						float f = (float) (ForceConstants.S4_REPULSE * Math.exp(c * dist));
						direction = direction.negate().mulLocal(f);
						mbody.applyForceToCenter(direction);
					}
				}
			}
		}
	}

	/**
	 * Calculates the squared distance between a point and a triangle in 2D
	 * space.
	 * @param y point
	 * @param <V0, V1, V2> the triangle
	 * @return squared distance between point and triangle
	 */
	Vec2 squaredDistance(Vec2 y, Vec2 v0, Vec2 v1, Vec2 v2) {
		// T has vertices V0, V1, V2
		// t0 = n0/d0 = Dot(Y - V0, V1 - V0) / Dot(V1 - V0, V1 - V0)
		Vec2 D0 = y.sub(v0), E0 = v1.sub(v0);
		float n0 = Vec2.dot(D0, E0);
		// t1 = n1/d1 = Vec2.dot(Y - V1, V2 - V1) / Vec2.dot(V2 - V1, V2 - V1)
		Vec2 D1 = y.sub(v1), E1 = v2.sub(v1);
		float n1 = Vec2.dot(D1, E1);
		if (n0 <= 0 && n1 <= 0) // closest point is V1
			return D1;
		// t2 = n2/d2 = Vec2.dot(Y - V2, V0 - V2) / Vec2.dot(V0 - V2, V0 - V2);
		Vec2 D2 = y.sub(v2), E2 = v0.sub(v2);
		float n2 = Vec2.dot(D2, E2);
		if (n1 <= 0 && n2 == 0) // closest point is V2
			return D2;
		if (n0 <= 0 && n2 <= 0) // closest point is V0
			return D0;

		// If this line is reached, the closest point lies on an edge, not on a vertex.
		float[] m = Barycentric(v0, v1, v2, y);
		
		if (m[0] < 0) { // closest point is V1 + t1 * E1
			float t1 = n1 / Vec2.dot(E1, E1);
			Vec2 D = v1.add(E1.mul(t1)).sub(y);
			return D;
		}
		if (m[1] < 0) { // closest point is V2 + t2 * E2
		    float e22 = Vec2.dot(E2, E2);
			float t2 = n2 / e22;
			Vec2 D = v2.add(E2.mul(t2)).sub(y);
			return D;
		}
		if (m[2] < 0) { // closest point is V0 + t0 * E0
		    float e00 = Vec2.dot(E0, E0);
			float t0 = n0 / e00;
			Vec2 D = v0.add(E0.mul(t0)).sub(y);
			return D; // RETURN 4a
		}

		return new Vec2(0, 0); // Y is inside triangle
	}
	
	// Compute barycentric coordinates (u, v, w) for
	// point p with respect to triangle (a, b, c)
	float[] Barycentric(Vec2 a, Vec2 b, Vec2 c, Vec2 p)
	{
	    Vec2 v0 = b.sub(a), v1 = c.sub(a), v2 = p.sub(a);
	    float e00 = Vec2.dot(v0, v0);
	    float e01 = Vec2.dot(v0, v1);
	    float e11 = Vec2.dot(v1, v1);
	    float e20 = Vec2.dot(v2, v0);
	    float e21 = Vec2.dot(v2, v1);
	    float denom = e00 * e11 - e01 * e01;
	    
	    float v = (e11 * e20 - e01 * e21) / denom;
	    float w = (e00 * e21 - e01 * e20) / denom;
	    float u = 1.0f - v - w;
	    
	    return new float[] {u, v, w};
	}
}
